package edu.gatech.cc.liam.geometry.linear.convexhull;

import java.io.BufferedReader;
import java.io.BufferedWriter;
import java.io.IOException;
import java.io.InputStreamReader;
import java.io.OutputStreamWriter;

import edu.gatech.cc.liam.geometry.linear.ApproximateEuclideanBall;
import edu.gatech.cc.liam.geometry.linear.NPoint;
import edu.gatech.cc.liam.geometry.linear.PointSet;
import edu.gatech.cc.liam.geometry.linear.Polytope;

public class QHalf {

	static final boolean PRINT_STREAMS = false;
	
	static public PointSet qHalfspaceHull(Polytope boundingHull) {
		return qHalfspaceHull(boundingHull.toMatrix());
	}
	static public PointSet qHalfspaceHull(Polytope boundingHull, NPoint interiorPoint) {
		return qHalfspaceHull(boundingHull.toMatrix(), interiorPoint);
	}
	static public PointSet qHalfspaceHull(double[][] boundingHull) {
		int dimensions = boundingHull[0].length-1; //boundingHull.getDimensions();
		return qHalfspaceHull(boundingHull, new NPoint(dimensions));
	}
	
	static public PointSet qHalfspaceHull(double[][] boundingHull, NPoint interiorPoint) {
		PointSet returnSet;
		if(interiorPoint.dimensions() >= 2) {
			returnSet = qHalfCall(boundingHull, interiorPoint, Preferences.QHALF_EXECUTABLE + " Fp");
		} else if (interiorPoint.dimensions() == 1) {
			returnSet = new PointSet();
			double[] minAndMax = findMaxAndMin(boundingHull);
			if(minAndMax[0] == minAndMax[1]) {
				double[] aPoint = new double[1];
				aPoint[0] = minAndMax[0];
				returnSet.add(new NPoint(aPoint));
			} else {
				double[] minPoint = new double[1];
				minPoint[0] = minAndMax[0];
				returnSet.add(new NPoint(minPoint));
				double[] maxPoint = new double[1];
				maxPoint[0] = minAndMax[1];
				returnSet.add(new NPoint(maxPoint));
			}
		} else {
			throw new Error("TOO FEW DIMENSIONS: Less than 1");
		}
		return returnSet;
	}

	static private double[] findMaxAndMin(double[][] inputHalfSpaces) {
		double min = Double.NEGATIVE_INFINITY;
		double max = Double.POSITIVE_INFINITY;
		for(int i = 0; i<inputHalfSpaces.length; i++) {
			if(inputHalfSpaces[i][0] > 0) {
				max = Math.min(max, -inputHalfSpaces[i][1] / inputHalfSpaces[i][0]);
			} else if(inputHalfSpaces[i][0] < 0) {
				min = Math.max(min, -inputHalfSpaces[i][1] / inputHalfSpaces[i][0]);
			} else {
				throw new Error("Bad inequality in 2 dimensions");
			}
		}
		if(Math.abs(max-min) < edu.gatech.cc.liam.geometry.linear.convexhull.Preferences.TOLERANCE)
			min = max;
		if(min > max) {
			assert false;
			return null;
		}
		double[] rVal = new double[2];
		rVal[0] = min;
		rVal[1] = max;
		return rVal;
	}
	
	static private PointSet qHalfCall(double[][] boundingHull, NPoint interiorPT, String executeString) {
		PointSet returnedPoints;
		try
		{
			Runtime rt = Runtime.getRuntime();
			Process p = rt.exec(Preferences.QHULL_LOCATION + Preferences.fileSeparator + executeString + " Tz") ;
			BufferedWriter input = new BufferedWriter(new OutputStreamWriter(p.getOutputStream()) );

			if(PRINT_STREAMS) {
				BufferedWriter input2 = new BufferedWriter(new OutputStreamWriter(System.out));
				sendInputString(boundingHull, interiorPT.values, input2);
				input2.flush();
			}
			sendInputString(boundingHull, interiorPT.values, input);
			input.close();

			BufferedReader output = new BufferedReader(new InputStreamReader(p.getInputStream()));
			returnedPoints = QHull.parseOutputAsPointSet(output);
			if(returnedPoints == null) {
				System.out.println("BAD RESULT in QHalf. Std-Error:");
				BufferedReader error = new BufferedReader(new InputStreamReader(p.getErrorStream()));
				String lineOfOutput;
				while ((lineOfOutput = error.readLine()) != null) {
					System.out.println(lineOfOutput);
				}
				System.out.println("Input:");
				BufferedWriter stdOut = new BufferedWriter(new OutputStreamWriter(System.out));
				sendInputString(boundingHull, interiorPT.values, stdOut);
				stdOut.flush();
				//System.out.println("Bad Input = \n" + badInput);
			}
			output.close();

			p.destroy();

		}catch(Exception exc){
			exc.printStackTrace();
			throw new Error();
		}
		//System.out.println(outputStr);
		return returnedPoints;
	}

	private static void sendInputString(double[][] boundingHull, double[] interiorPT, BufferedWriter input) throws IOException {
		Integer dim = boundingHull[0].length-1; //boundingHull.getDimensions();
		input.write( dim + " 1\n");
		for (double d : interiorPT) {
			input.write(d + " ");
		}
		input.write("\n");
//		input.write( (dim+1) + "\n" + boundingHull.size() + "\n");
//		for (Halfspace hp : boundingHull){
//			for (double d : hp.normal) {
//				input.write(d + " ");
//			}
//			input.write(hp.offset + "\n");
//		}
		input.write( (dim+1) + "\n" + boundingHull.length + "\n");
		for (double[] hp : boundingHull){
			for (double d : hp) {
				input.write(d + " ");
			}
			input.write("\n");
		}
//		input.write( (dim+1) + "\n" + boundingHull.length + "\n");
//		for (int i=0; i<boundingHull.length; i++){
//			for (int j=0; j<boundingHull[i].length; j++) {
//				input.write(boundingHull[i][j] + " ");
//			}
//			input.write("\n");
//		}
	}
	
	public static void main(String[] args) {
		System.out.println("Testing Cube:");
		ApproximateEuclideanBall testBall = new ApproximateEuclideanBall(3, 1);
		System.out.println("ball normals: " + testBall);
		Polytope aCHull = new Polytope(testBall, -1.0);
		aCHull.get(0).offset *= 2;
		System.out.println("ball hull: " + aCHull);
		PointSet aCSet = aCHull.getExtremePoints();
		System.out.println("ball vertices: " + aCSet);
		
		System.out.println("\nQHalf doesn't work when the interior point is not clearly inside object:");
		testBall = new ApproximateEuclideanBall(3, 1);
		System.out.println("ball normals: " + testBall);
		aCHull = new Polytope(testBall, -1.0);
		aCHull.get(0).offset *= 0.0;
		System.out.println("ball hull: " + aCHull);
		aCSet = aCHull.getExtremePoints();
		System.out.println("ball vertices: " + aCSet);
		
		System.out.println("\nQHalf doesn't work when the hull lies in a lower dimension");
		testBall = new ApproximateEuclideanBall(3, 1);
		System.out.println("ball normals: " + testBall);
		aCHull = new Polytope(testBall, -1.0);
		aCHull.get(0).offset *= 0.0;
		aCHull.get(5).offset *= 0.0;
		System.out.println("ball hull: " + aCHull);
		aCSet = aCHull.getExtremePoints();
		System.out.println("ball vertices: " + aCSet);
		
		System.out.println("\nBut the convexHull class works around this by projecting into the lower dimension first");
	}
	
}
